/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "main_functions.h"

#include "detection_responder.h"
#include "image_provider.h"
#include "model_settings.h"
#include "tensorflow/lite/micro/micro_error_reporter.h"
#include "tensorflow/lite/micro/micro_interpreter.h"
#include "tensorflow/lite/micro/micro_mutable_op_resolver.h"
#include "tensorflow/lite/micro/models/person_detect_model_data.h"
#include "tensorflow/lite/micro/system_setup.h"
#include "tensorflow/lite/schema/schema_generated.h"
#include "testdata/person_image_data.h"
#include "testdata/no_person_image_data.h"
#include "tensorflow/lite/micro/all_ops_resolver.h"
#include "yolo_layer.h"
#include "test_lcd_data.h"
#include "person_160.h"

#include "tos_k.h"
// Globals, used for compatibility with Arduino-style sketches.
namespace {
tflite::ErrorReporter* error_reporter = nullptr;
const tflite::Model* model = nullptr;
tflite::MicroInterpreter* interpreter = nullptr;
TfLiteTensor* input = nullptr;

// In order to use optimized tensorflow lite kernels, a signed int8_t quantized
// model is preferred over the legacy unsigned model format. This means that
// throughout this project, input images must be converted from unisgned to
// signed format. The easiest and quickest way to convert from unsigned to
// signed 8-bit integers is to subtract 128 from the unsigned value to get a
// signed value.
static int bgr_mode = 1;
#define OPTIMIZE __attribute__((section(".ramfunc.$SRAM_ITC")))
OPTIMIZE void IMAGE_Resize565(uint16_t* srcData, int srcWidth, int srcHeight,
                  uint8_t* dstData, int dstWidth, int dstHeight, int channels)
{
    double dx = 1.0 * dstWidth / srcWidth;
    double dy = 1.0 * dstHeight / srcHeight;

    int step = channels * srcWidth;
    int scale_step = channels * dstWidth;

    int i, j, k, pre_i, pre_j, after_i, after_j;

    if (channels == 1)
    {
        for (i = 0; i < dstHeight; i++)
        {
            for (j = 0; j < dstWidth; j++)
            {
                dstData[(dstHeight - 1 - i) * scale_step + j] = 0;
            }
        }

        for (i = 0; i < dstHeight; i++)
        {
            for (j = 0; j < dstWidth; j++)
            {
              after_i = i;
              after_j = j;
              pre_i = (int)(after_i / dy + 0);
              pre_j = (int)(after_j / dx + 0);
              if (pre_i >= 0 && pre_i < srcHeight && pre_j >= 0 && pre_j < srcWidth)
              {
                  dstData[i * scale_step + j] = srcData[pre_i * step + pre_j];
              }
            }
        }
    }
    else if (channels == 3)
    {
        for (i = 0; i < dstHeight; i++)
        {
            for (j = 0; j < dstWidth; j++)
            {
                after_i = i;
                after_j = j;
                pre_i = (int)(after_i / dy + 0.5);
                pre_j = (int)(after_j / dx + 0.5);
                if (pre_i >= 0 && pre_i < srcHeight && pre_j >= 0 && pre_j < srcWidth)
                {
					uint16_t rgb565 = srcData[pre_i * step / channels + pre_j];
					uint8_t r = ((rgb565 >> 11) << 3) & (0xff);
					uint8_t g = ((rgb565 >> 5) << 2) & (0xff);
					uint8_t b = (rgb565 << 3) & (0xff);
					if(bgr_mode){
						dstData[i * scale_step + j * 3 + 0] = b;
						dstData[i * scale_step + j * 3 + 1] = g;
						dstData[i * scale_step + j * 3 + 2] = r;
					}else{
						dstData[i * scale_step + j * 3 + 0] = r;
						dstData[i * scale_step + j * 3 + 1] = g;
						dstData[i * scale_step + j * 3 + 2] = b;
					}
                }
            }
        }
    }
}

void rgb5652gray(uint16_t *src, unsigned char *dst, int width, int height)
{
    int r, g, b;
    for (int i=0; i<width*height; ++i)
    {
        r=((*(src+i))>>11)&0x1F;//  11 1111
        g=((*(src+i))>>5)&0x3F;
        b=((*(src+i))>>0)&0x1F;
        // build weighted average:
        *dst++ = (r * 76 + g * 150 + b * 30) >> 8;
    }
}

int is_in_array(short x, short y, short height, short width)
{
    if (x >= 0 && x < width && y >= 0 && y < height)
        return 1;
    else
        return 0;
}

void bilinera_interpolation(uint8_t* in_array, short height, short width,
                            uint8_t* out_array, short out_height, short out_width)
{
    double h_times = (double)out_height / (double)height,
           w_times = (double)out_width / (double)width;
    short  x1, y1, x2, y2, f11, f12, f21, f22;
    double x, y;

    for (int i = 0; i < out_height; i++){
        for (int j = 0; j < out_width; j++){
            x = j / w_times;
            y = i / h_times;

            x1 = (short)(x - 1);
            x2 = (short)(x + 1);
            y1 = (short)(y + 1);
            y2 = (short)(y - 1);
            f11 = is_in_array(x1, y1, height, width) ? in_array[y1*width+x1] : 0;
            f12 = is_in_array(x1, y2, height, width) ? in_array[y2*width+x1] : 0;
            f21 = is_in_array(x2, y1, height, width) ? in_array[y1*width+x2] : 0;
            f22 = is_in_array(x2, y2, height, width) ? in_array[y2*width+x2] : 0;
            out_array[i*out_width+j] = (uint8_t)(((f11 * (x2 - x) * (y2 - y)) +
                                       (f21 * (x - x1) * (y2 - y)) +
                                       (f12 * (x2 - x) * (y - y1)) +
                                       (f22 * (x - x1) * (y - y1))) / ((x2 - x1) * (y2 - y1)));
        }
    }
}
#define RGB565_RED      0xf800
#define RGB565_GREEN    0x07e0
#define RGB565_BLUE     0x001f

uint16_t rgb565= 0X7DEF;
uint8_t gray_320[320*240]={0};
uint8_t input_gray_160[160*160]={0};

// An area of memory to use for input, output, and intermediate arrays.
constexpr int kTensorArenaSize = 150 * 1024;
static uint8_t tensor_arena[kTensorArenaSize];
}  // namespace

// The name of this function is important for Arduino compatibility.
void setup() {
  tflite::InitializeTarget();

  // Set up logging. Google style is to avoid globals or statics because of
  // lifetime uncertainty, but since this has a trivial destructor it's okay.
  // NOLINTNEXTLINE(runtime-global-variables)
  static tflite::MicroErrorReporter micro_error_reporter;
  error_reporter = &micro_error_reporter;

  // Map the model into a usable data structure. This doesn't involve any
  // copying or parsing, it's a very lightweight operation.
  model = tflite::GetModel(g_person_detect_model_data);
  if (model->version() != TFLITE_SCHEMA_VERSION) {
    TF_LITE_REPORT_ERROR(error_reporter,
                         "Model provided is schema version %d not equal "
                         "to supported version %d.",
                         model->version(), TFLITE_SCHEMA_VERSION);
    return;
  }

  // Pull in only the operation implementations we need.
  // This relies on a complete list of all the ops needed by this graph.
  // An easier approach is to just use the AllOpsResolver, but this will
  // incur some penalty in code space for op implementations that are not
  // needed by this graph.
  static tflite::MicroMutableOpResolver<8> micro_op_resolver;
  micro_op_resolver.AddAdd();
  micro_op_resolver.AddConcatenation();
  micro_op_resolver.AddConv2D();
  micro_op_resolver.AddDepthwiseConv2D();
  micro_op_resolver.AddQuantize();
  micro_op_resolver.AddPad();
  micro_op_resolver.AddDequantize();

  //static tflite::AllOpsResolver micro_op_resolver;
  // Build an interpreter to run the model with.
  // NOLINTNEXTLINE(runtime-global-variables)
  static tflite::MicroInterpreter static_interpreter(
      model, micro_op_resolver, tensor_arena, kTensorArenaSize, error_reporter);
  interpreter = &static_interpreter;

  // Allocate memory from the tensor_arena for the model's tensors.
  TfLiteStatus allocate_status = interpreter->AllocateTensors();
  if (allocate_status != kTfLiteOk) {
    TF_LITE_REPORT_ERROR(error_reporter, "AllocateTensors() failed");
    return;
  }

  // Get information about the memory area to use for the model's input.
  input = interpreter->input(0);
  PRINTF("AI model init success\n");
}
// The name of this function is important for Arduino compatibility.
uint8_t data_test_gray[160*160]={0};
float data_test_float[160*160]={0.0};

float* loop() {
    for(int i=0 ; i<320*240; i++)
    {
        uint8_t R = (((uint16_t*)gImage_test_lcd_data)[i] & RGB565_RED) >> 8;
        uint8_t G = (((uint16_t*)gImage_test_lcd_data)[i] & RGB565_GREEN) >> 3;
        uint8_t B = (((uint16_t*)gImage_test_lcd_data)[i] & RGB565_BLUE) << 3;
        uint8_t Gray = (R*30 + G*59 + B*11 + 50) / 100;
        gray_320[i] = Gray;
    }
    bilinera_interpolation(gray_320, 240, 320, input_gray_160, 160, 160);
	for(int i=0 ; i<160*160 ; i++)
	{
		data_test_float[i]=input_gray_160[i]/256.0;
		//data_test_float[i] = (data_test_float[i]/128.0)-1.0;
		//data_test_float[i]=0.0;
	}

	PRINTF("\n............................\n");
	PRINTF("input information:\n");
	PRINTF("input type is %d (float32)\n" , input->type);
	PRINTF("input dim is %d\n" , input->dims->size);
	PRINTF("input type is %d %d %d %d\n" , input->dims->data[0] , input->dims->data[1], input->dims->data[2], input->dims->data[3]);
	memcpy(input->data.f , data_test_float , 160*160);

  // Run the model on this input and make sure it succeeds.
  if (kTfLiteOk != interpreter->Invoke()) {
    TF_LITE_REPORT_ERROR(error_reporter, "Invoke failed.");
  }
  TfLiteTensor* output = interpreter->output(0);
  PRINTF("\n............................\n");
  PRINTF("output information:\n");
  PRINTF("output type is %d (float32)\n" , output->type);
  PRINTF("output dim is %d\n" , output->dims->size);
  PRINTF("output type is %d %d %d %d\n" , output->dims->data[0] , output->dims->data[1], output->dims->data[2], output->dims->data[3]);
  // Process the inference results.
  /*
  PRINTF("output data\n");
  for(int i=0 ; i<interpreter->outputs_size() ; i++)
  {
	  if(output->data.f[i] >=0)
		  PRINTF("%f " , output->data.f[i]);
	  else
		  PRINTF("-%f " , output->data.f[i]);
  }
  PRINTF("\n............................\n");
  */
  PRINTF("AI model run success\n");
  return (float*)output->data.f;
}
